/*
 * translation_matching.cc
 *
 *  Created on: Oct 22, 2011
 *      Author: tqlong
 */
#include <boost/foreach.hpp>

#include "translation_matching.h"

void NaiveTranslationMatching::mark(cv::Mat& l_r, const cv::Mat& lTrans, const cv::Mat& rTrans)
{
  BOOST_FOREACH(const map_value& p, l2r_)
  {
    int l = p.first, r = p.second;
    const arma::vec& lvec = lList_[l], &rvec = rList_[r];
    cv::Mat lp = (cv::Mat_<double>(3,1) << lvec(0), lvec(1), 1),
            rp = (cv::Mat_<double>(3,1) << rvec(0), rvec(1), 1);
    cv::Mat lp_t_m = lTrans * lp, rp_t_m = rTrans * rp;
    cv::Point2d lp_t(lp_t_m.at<double>(0,0), lp_t_m.at<double>(1,0)),
        rp_t(rp_t_m.at<double>(0,0), rp_t_m.at<double>(1,0));

    cv::line(l_r, lp_t, rp_t, cv::Scalar(0,255,255), 1);
  }
}

void NaiveTranslationMatching::match(const FeatureList& lList, const FeatureList& rList)
{
  lList_ = lList;
  rList_ = rList;

  int max_match = 0;

  for (unsigned int i = 0; i < lList_.size(); i++)
  {
    const arma::vec& lvec = lList_[i];
    for (unsigned int j = 0; j < rList_.size(); j++)
    {
      const arma::vec& rvec = rList_[j];
      double tx = rvec(0) - lvec(0), ty = rvec(1) - lvec(1);
      cv::Mat transform = (cv::Mat_<double>(2,3) << 1, 0, tx, 0, 1, ty);
      int match = count_match(transform);
      std::cerr << i << " (" << lvec(0) << ", " << lvec(1) << ") --> "
                << j << " (" << rvec(0) << ", " << rvec(1) << ") : "
                << match << std::endl;
      if (match > max_match)
      {
        max_match = match;
        max_transform_ = transform;
      }
    }
  }

  if (max_match > 0)
    do_match(max_transform_);
}

int NaiveTranslationMatching::count_match(const cv::Mat& transform)
{
  std::map<int, bool> used;
  int count = 0;
  BOOST_FOREACH(const Feature& lf, lList_)
  {
    const arma::vec& lvec = lf;
    cv::Point2d lp(lvec(0), lvec(1)), lp_t = this->transform(transform, lp);
    double min_dist = std::numeric_limits<double>::infinity();
    int min_j = -1;
    for (unsigned int j = 0; j < rList_.size(); j++) if (used.find(j) == used.end())  // the right feature point is not used
    {
      const arma::vec& rvec = rList_[j];
      cv::Point2d rp(rvec(0), rvec(1));
      double distance = cv::norm(lp_t - rp);
      if (distance < min_dist && distance < tol_)
      {
        min_dist = distance;
        min_j = j;
      }
    }
    if (min_j != -1)
    {
      used[min_j] = true;
      count++;
    }
  }
  return count;
}

void NaiveTranslationMatching::do_match(const cv::Mat& transform)
{
  std::map<int, bool> used;
  int count = 0;
  for (unsigned int i = 0; i < lList_.size(); i++)
  {
    const arma::vec& lvec = lList_[i];
    cv::Point2d lp(lvec(0), lvec(1)), lp_t = this->transform(transform, lp);
    double min_dist = std::numeric_limits<double>::infinity();
    int min_j = -1;
    for (unsigned int j = 0; j < rList_.size(); j++) if (used.find(j) == used.end())  // the right feature point is not used
    {
      const arma::vec& rvec = rList_[j];
      cv::Point2d rp(rvec(0), rvec(1));
      double distance = cv::norm(lp_t - rp);
      if (distance < min_dist && distance < tol_)
      {
        min_dist = distance;
        min_j = j;
      }
    }
    if (min_j != -1)
    {
      used[min_j] = true;
      count++;
      l2r_[i] = min_j;
      r2l_[min_j] = i;
    }
  }
}

cv::Point2d NaiveTranslationMatching::transform(const cv::Mat& transform, cv::Point2d p)
{
  cv::Mat p_mat = (cv::Mat_<double>(3,1) << p.x, p.y, 1);
  cv::Mat trans_p_mat = transform * p_mat;
  return cv::Point2d (trans_p_mat.at<double>(0,0), trans_p_mat.at<double>(1,0));
}
